
clear all; clc;

global m S CD g rou0 Hre RE

%% Parameter Declaration

  v_ini =   300            ; % m/s
the_ini =   0   / 180 * pi ; % rad
  x_ini =   0              ; % m
  y_ini =   2500           ; % m
  
  x_fin =   5000           ; % m
  y_fin =   0              ; % m
the_upp = - 80  / 180 * pi ; % rad
the_low = - 90  / 180 * pi ; % rad
col_upp =   60             ; % m/s2
col_low = - 60             ; % m/s2

  m     =   200            ; % kg
  S     =   0.04           ; % m2
  CD    =   1              ;
  g     =   9.8            ; % m /s2
rou0    =   1.255          ; % kg/m3
Hre     =   7.6  * 1e3     ; % m
 RE     =   6378 * 1e3     ; % m

 t0     =   0              ; % s
 tf     =   30             ; % s
 
%% Numerical Settings

X_ini = [ v_ini ; the_ini ; x_ini ; y_ini ] ;

N     =  100            ; % number of phases
lmax  =  15             ;
eps   =  1e-3 ;
h     = ( tf - t0 ) / N ;

[ ta , Xa_0 ] = RULER( @Dynamics,X_ini,t0,tf,N ) ;

X = Xa_0 ;
a = zeros(1,N+1) ;

%% Sequential CVX Optimization

for l = 1:lmax
   
    XX = X ;
    aa = a ;
    
    cvx_solver mosek
    cvx_begin
    
    variables X(4,N+1) a(1,N+1) yita
     
    minimize ( sum(abs(a)) + yita )
    
    subject to
    
    X(:,1)   ==   X_ini    ;
    X(2,end) <=   the_upp  ;
    the_low  <=   X(2,end) ;
    X(3,end) ==   x_fin    ;
    X(4,end) ==   y_fin    ;

    for i = 1 : N+1
    a(:,i)  <= col_upp ;
    col_low <= a(:,i)  ;
    norm(a(:,i)-aa(:,i)) <= yita ;
    norm(X(:,i)-XX(:,i)) <= yita ;
    end
    
    for i = 2 : N+1
    
    [ Ak , Bk , Fk ] = Coeff( XX(:,i-1) , aa(:,i-1) ) ;
        
    dy = Fk + Ak * ( X(:,i-1) - XX(:,i-1) ) +...
         Bk * ( a(:,i-1) - aa(:,i-1) ) ;
    
    X(:,i) == X(:,i-1) + h * dy ;

    end

    
    cvx_end
    
    if yita < eps
        break;
    end
    
    [ l , yita ]
    
end
    



%% Data Extraction and Plot Figures

   ll = 1 ;   

   figure(ll)
   hold on
   grid on
   box on
   xlabel('T/s');
   ylabel('V/ m/s');
   plot(ta,X(ll,:),'LineWidth',1.5);
   title('速度曲线');
   ll = ll +1 ;
   axis([0,30,180,300]);

   figure(ll)
   hold on
   grid on
   box on
   xlabel('T/s');
   ylabel('theta/ rad');
   plot(ta,X(ll,:),'LineWidth',1.5);
   title('弹道倾角曲线')
   ll = ll +1 ;
   axis([0,30,-1.6,0.4]);
   
   figure(ll)
   hold on
   grid on
   box on
   xlabel('T/s');
   ylabel('x/ m');
   plot(ta,X(ll,:),'LineWidth',1.5);
   ll = ll +1 ;
   
   figure(ll)
   hold on
   grid on
   box on
   xlabel('T/s');
   ylabel('y/ m');
   plot(ta,X(ll,:),'LineWidth',1.5);
   ll = ll +1 ;
   
figure(ll)
   hold on
   grid on
   box on
   xlabel('T/s');
   ylabel('control/ m/s2');
   plot(ta,a(:,:),'LineWidth',1.5);
   title('控制力加速度曲线');
   axis([0,30,-60,60]);
   
   figure;
   hold on
   grid on
   box on
   xlabel('x/ m');
   ylabel('y/ m');
   plot(X(3,:),X(4,:),'LineWidth',1.5);
   title('弹道');